// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2019
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <LowLevel/GteLogger.h>
#include <LowLevel/GteMinHeap.h>
#include <array>
#include <map>
#include <memory>
#include <set>
#include <stack>

// Extract the minimal cycle basis for a planar graph.  The input vertices and
// edges must form a graph for which edges intersect only at vertices; that is,
// no two edges must intersect at an interior point of one of the edges.  The
// algorithm is described in 
//   http://www.geometrictools.com/Documentation/MinimalCycleBasis.pdf
// The graph might have filaments, which are polylines in the graph that are
// not shared by a cycle.  These are also extracted by the implementation.
// Because the inputs to the constructor are vertices and edges of the graph,
// isolated vertices are ignored.
//
// The computations that determine which adjacent vertex to visit next during
// a filament or cycle traversal do not require division, so the exact
// arithmetic type BSNumber<UIntegerAP32> suffices for ComputeType when you
// want to ensure a correct output.  (Floating-point rounding errors
// potentially can lead to an incorrect output.)

namespace gte
{

template <typename Real>
class MinimalCycleBasis
{
public:
    struct Tree
    {
        std::vector<int> cycle;
        std::vector<std::shared_ptr<Tree>> children;
    };

    // The input positions and edges must form a planar graph for which edges
    // intersect only at vertices; that is, no two edges must intersect at an
    // interior point of one of the edges.
    MinimalCycleBasis(
        std::vector<std::array<Real, 2>> const& positions,
        std::vector<std::array<int, 2>> const& edges,
        std::vector<std::shared_ptr<Tree>>& forest);

private:
    // No copy or assignment allowed.
    MinimalCycleBasis(MinimalCycleBasis const&) = delete;
    MinimalCycleBasis& operator=(MinimalCycleBasis const&) = delete;

    struct Vertex
    {
        Vertex(int inName, std::array<Real, 2> const* inPosition);
        bool operator< (Vertex const& vertex) const;

        // The index into the 'positions' input provided to the call to
        // operator().  The index is used when reporting cycles to the
        // caller of the constructor for MinimalCycleBasis.
        int name;

        // Multiple vertices can share a position during processing of
        // graph components.
        std::array<Real, 2> const* position;

        // The mVertexStorage member owns the Vertex objects and maintains
        // the reference counts on those objects.  The adjacent pointers
        // are considered to be weak pointers; neither object ownership nor
        // reference counting is required by 'adjacent'.
        std::set<Vertex*> adjacent;

        // Support for depth-first traversal of a graph.
        int visited;
    };

    // The constructor uses GetComponents(...) and DepthFirstSearch(...) to
    // get the connected components of the graph implied by the input 'edges'.
    // Recursive processing uses only DepthFirstSearch(...) to collect
    // vertices of the subgraphs of the original graph.
    static void DepthFirstSearch(Vertex* vInitial, std::vector<Vertex*>& component);

    // Support for traversing a simply connected component of the graph.
    std::shared_ptr<Tree> ExtractBasis(std::vector<Vertex*>& component);
    void RemoveFilaments(std::vector<Vertex*>& component);
    std::shared_ptr<Tree> ExtractCycleFromComponent(std::vector<Vertex*>& component);
    std::shared_ptr<Tree> ExtractCycleFromClosedWalk(std::vector<Vertex*>& closedWalk);
    std::vector<int> ExtractCycle(std::vector<Vertex*>& closedWalk);

    Vertex* GetClockwiseMost(Vertex* vPrev, Vertex* vCurr) const;
    Vertex* GetCounterclockwiseMost(Vertex* vPrev, Vertex* vCurr) const;

    // Storage for referenced vertices of the original graph and for new
    // vertices added during graph traversal.
    std::vector<std::shared_ptr<Vertex>> mVertexStorage;
};


template <typename Real>
MinimalCycleBasis<Real>::MinimalCycleBasis(std::vector<std::array<Real, 2>> const& positions,
    std::vector<std::array<int, 2>> const& edges, std::vector<std::shared_ptr<Tree>>& forest)
{
    forest.clear();
    if (positions.size() == 0 || edges.size() == 0)
    {
        // The graph is empty, so there are no filaments or cycles.
        return;
    }

    // Determine the unique positions referenced by the edges.
    std::map<int, std::shared_ptr<Vertex>> unique;
    for (auto const& edge : edges)
    {
        for (int i = 0; i < 2; ++i)
        {
            int name = edge[i];
            if (unique.find(name) == unique.end())
            {
                std::shared_ptr<Vertex> vertex =
                    std::make_shared<Vertex>(name, &positions[name]);
                unique.insert(std::make_pair(name, vertex));

            }
        }
    }

    // Assign responsibility for ownership of the Vertex objects.
    std::vector<Vertex*> vertices;
    mVertexStorage.reserve(unique.size());
    vertices.reserve(unique.size());
    for (auto const& element : unique)
    {
        mVertexStorage.push_back(element.second);
        vertices.push_back(element.second.get());
    }

    // Determine the adjacencies from the edge information.
    for (auto const& edge : edges)
    {
        auto iter0 = unique.find(edge[0]);
        auto iter1 = unique.find(edge[1]);
        iter0->second->adjacent.insert(iter1->second.get());
        iter1->second->adjacent.insert(iter0->second.get());
    }

    // Get the connected components of the graph.  The 'visited' flags are
    // 0 (unvisited), 1 (discovered), 2 (finished).  The Vertex constructor
    // sets all 'visited' flags to 0.
    std::vector<std::vector<Vertex*>> components;
    for (auto vInitial : mVertexStorage)
    {
        if (vInitial->visited == 0)
        {
            components.push_back(std::vector<Vertex*>());
            DepthFirstSearch(vInitial.get(), components.back());
        }
    }

    // The depth-first search is used later for collecting vertices for
    // subgraphs that are detached from the main graph, so the 'visited'
    // flags must be reset to zero after component finding.
    for (auto vertex : mVertexStorage)
    {
        vertex->visited = 0;
    }

    // Get the primitives for the components.
    for (auto& component : components)
    {
        forest.push_back(ExtractBasis(component));
    }
}

template <typename Real>
void MinimalCycleBasis<Real>::DepthFirstSearch(Vertex* vInitial, std::vector<Vertex*>& component)
{
    std::stack<Vertex*> vStack;
    vStack.push(vInitial);
    while (vStack.size() > 0)
    {
        Vertex* vertex = vStack.top();
        vertex->visited = 1;
        size_t i = 0;
        for (auto adjacent : vertex->adjacent)
        {
            if (adjacent && adjacent->visited == 0)
            {
                vStack.push(adjacent);
                break;
            }
            ++i;
        }

        if (i == vertex->adjacent.size())
        {
            vertex->visited = 2;
            component.push_back(vertex);
            vStack.pop();
        }
    }
}

template <typename Real>
std::shared_ptr<typename MinimalCycleBasis<Real>::Tree>
    MinimalCycleBasis<Real>::ExtractBasis(std::vector<Vertex*>& component)
{
    // The root will not have its 'cycle' member set.  The children are
    // the cycle trees extracted from the component.
    std::shared_ptr<Tree> tree = std::make_shared<Tree>();
    while (component.size() > 0)
    {
        RemoveFilaments(component);
        if (component.size() > 0)
        {
            tree->children.push_back(ExtractCycleFromComponent(component));
        }
    }

    if (tree->cycle.size() == 0 && tree->children.size() == 1)
    {
        // Replace the parent by the child to avoid having two empty
        // cycles in parent/child.
        std::shared_ptr<Tree> child = tree->children.back();
        tree->cycle = std::move(child->cycle);
        tree->children = std::move(child->children);
    }
    return tree;
}

template <typename Real>
void MinimalCycleBasis<Real>::RemoveFilaments(std::vector<Vertex*>& component)
{
    // Locate all filament endpoints, which are vertices, each having exactly
    // one adjacent vertex.
    std::vector<Vertex*> endpoints;
    for (auto vertex : component)
    {
        if (vertex->adjacent.size() == 1)
        {
            endpoints.push_back(vertex);
        }
    }

    if (endpoints.size() > 0)
    {
        // Remove the filaments from the component.  If a filament has two
        // endpoints, each having one adjacent vertex, the adjacency set of
        // the final visited vertex become empty.  We must test for that
        // condition before starting a new filament removal.
        for (auto vertex : endpoints)
        {
            if (vertex->adjacent.size() == 1)
            {
                // Traverse the filament and remove the vertices.
                while (vertex->adjacent.size() == 1)
                {
                    // Break the connection between the two vertices.
                    Vertex* adjacent = *vertex->adjacent.begin();
                    adjacent->adjacent.erase(vertex);
                    vertex->adjacent.erase(adjacent);

                    // Traverse to the adjacent vertex.
                    vertex = adjacent;
                }
            }
        }

        // At this time the component is either empty (it was a union of
        // polylines) or it has no filaments and at least one cycle.  Remove
        // the isolated vertices generated by filament extraction.
        std::vector<Vertex*> remaining;
        remaining.reserve(component.size());
        for (auto vertex : component)
        {
            if (vertex->adjacent.size() > 0)
            {
                remaining.push_back(vertex);
            }
        }
        component = std::move(remaining);
    }
}

template <typename Real>
std::shared_ptr<typename MinimalCycleBasis<Real>::Tree>
    MinimalCycleBasis<Real>::ExtractCycleFromComponent(std::vector<Vertex*>& component)
{
    // Search for the left-most vertex of the component.  If two or more
    // vertices attain minimum x-value, select the one that has minimum
    // y-value.
    Vertex* minVertex = component[0];
    for (auto vertex : component)
    {
        if (*vertex->position < *minVertex->position)
        {
            minVertex = vertex;
        }
    }

    // Traverse the closed walk, duplicating the starting vertex as the
    // last vertex.
    std::vector<Vertex*> closedWalk;
    Vertex* vCurr = minVertex;
    Vertex* vStart = vCurr;
    closedWalk.push_back(vStart);
    Vertex* vAdj = GetClockwiseMost(nullptr, vStart);
    while (vAdj != vStart)
    {
        closedWalk.push_back(vAdj);
        Vertex* vNext = GetCounterclockwiseMost(vCurr, vAdj);
        vCurr = vAdj;
        vAdj = vNext;
    }
    closedWalk.push_back(vStart);

    // Recursively process the closed walk to extract cycles.
    std::shared_ptr<Tree> tree = ExtractCycleFromClosedWalk(closedWalk);

    // The isolated vertices generated by cycle removal are also removed from
    // the component.
    std::vector<Vertex*> remaining;
    remaining.reserve(component.size());
    for (auto vertex : component)
    {
        if (vertex->adjacent.size() > 0)
        {
            remaining.push_back(vertex);
        }
    }
    component = std::move(remaining);

    return tree;
}

template <typename Real>
std::shared_ptr<typename MinimalCycleBasis<Real>::Tree>
    MinimalCycleBasis<Real>::ExtractCycleFromClosedWalk(std::vector<Vertex*>& closedWalk)
{
    std::shared_ptr<Tree> tree = std::make_shared<Tree>();

    std::map<Vertex*, int> duplicates;
    std::set<int> detachments;
    int numClosedWalk = static_cast<int>(closedWalk.size());
    for (int i = 1; i < numClosedWalk - 1; ++i)
    {
        auto diter = duplicates.find(closedWalk[i]);
        if (diter == duplicates.end())
        {
            // We have not yet visited this vertex.
            duplicates.insert(std::make_pair(closedWalk[i], i));
            continue;
        }

        // The vertex has been visited previously.  Collapse the closed walk
        // by removing the subwalk sharing this vertex.  Note that the vertex
        // is pointed to by closedWalk[diter->second] and closedWalk[i].
        int iMin = diter->second, iMax = i;
        detachments.insert(iMin);
        for (int j = iMin + 1; j < iMax; ++j)
        {
            Vertex* vertex = closedWalk[j];
            duplicates.erase(vertex);
            detachments.erase(j);
        }
        closedWalk.erase(closedWalk.begin() + iMin + 1, closedWalk.begin() + iMax + 1);
        numClosedWalk = static_cast<int>(closedWalk.size());
        i = iMin;
    }

    if (numClosedWalk > 3)
    {
        // We do not know whether closedWalk[0] is a detachment point.  To
        // determine this, we must test for any edges strictly contained by
        // the wedge formed by the edges <closedWalk[0],closedWalk[N-1]> and
        // <closedWalk[0],closedWalk[1]>.  However, we must execute this test
        // even for the known detachment points.  The ensuing logic is designed
        // to handle this and reduce the amount of code, so we insert
        // closedWalk[0] into the detachment set and will ignore it later if
        // it actually is not.
        detachments.insert(0);

        // Detach subgraphs from the vertices of the cycle.
        for (auto i : detachments)
        {
            Vertex* original = closedWalk[i];
            Vertex* maxVertex = closedWalk[i + 1];
            Vertex* minVertex = (i > 0 ? closedWalk[i - 1] : closedWalk[numClosedWalk - 2]);

            std::array<Real, 2> dMin, dMax;
            for (int j = 0; j < 2; ++j)
            {
                dMin[j] = (*minVertex->position)[j] - (*original->position)[j];
                dMax[j] = (*maxVertex->position)[j] - (*original->position)[j];
            }

            bool isConvex = (dMax[0] * dMin[1] >= dMax[1] * dMin[0]); (void)isConvex;

            std::set<Vertex*> inWedge;
            std::set<Vertex*> adjacent = original->adjacent;
            for (auto vertex : adjacent)
            {
                if (vertex->name == minVertex->name || vertex->name == maxVertex->name)
                {
                    continue;
                }

                std::array<Real, 2> dVer;
                for (int j = 0; j < 2; ++j)
                {
                    dVer[j] = (*vertex->position)[j] - (*original->position)[j];
                }

                bool containsVertex;
                if (isConvex)
                {
                    containsVertex =
                        dVer[0] * dMin[1] > dVer[1] * dMin[0] &&
                        dVer[0] * dMax[1] < dVer[1] * dMax[0];
                }
                else
                {
                    containsVertex =
                        (dVer[0] * dMin[1] > dVer[1] * dMin[0]) ||
                        (dVer[0] * dMax[1] < dVer[1] * dMax[0]);
                }
                if (containsVertex)
                {
                    inWedge.insert(vertex);
                }
            }

            if (inWedge.size() > 0)
            {
                // The clone will manage the adjacents for 'original' that lie
                // inside the wedge defined by the first and last edges of the
                // subgraph rooted at 'original'.  The sorting is in the
                // clockwise direction.
                std::shared_ptr<Vertex> clone =
                    std::make_shared<Vertex>(original->name, original->position);
                mVertexStorage.push_back(clone);

                // Detach the edges inside the wedge.
                for (auto vertex : inWedge)
                {
                    original->adjacent.erase(vertex);
                    vertex->adjacent.erase(original);
                    clone->adjacent.insert(vertex);
                    vertex->adjacent.insert(clone.get());
                }

                // Get the subgraph (it is a single connected component).
                std::vector<Vertex*> component;
                DepthFirstSearch(clone.get(), component);

                // Extract the cycles of the subgraph.
                tree->children.push_back(ExtractBasis(component));
            }
            // else the candidate was closedWalk[0] and it has no subgraph
            // to detach.
        }

        tree->cycle = std::move(ExtractCycle(closedWalk));
    }
    else
    {
        // Detach the subgraph from vertex closedWalk[0]; the subgraph
        // is attached via a filament.
        Vertex* original = closedWalk[0];
        Vertex* adjacent = closedWalk[1];

        std::shared_ptr<Vertex> clone =
            std::make_shared<Vertex>(original->name, original->position);
        mVertexStorage.push_back(clone);

        original->adjacent.erase(adjacent);
        adjacent->adjacent.erase(original);
        clone->adjacent.insert(adjacent);
        adjacent->adjacent.insert(clone.get());

        // Get the subgraph (it is a single connected component).
        std::vector<Vertex*> component;
        DepthFirstSearch(clone.get(), component);

        // Extract the cycles of the subgraph.
        tree->children.push_back(ExtractBasis(component));
        if (tree->cycle.size() == 0 && tree->children.size() == 1)
        {
            // Replace the parent by the child to avoid having two empty
            // cycles in parent/child.
            std::shared_ptr<Tree> child = tree->children.back();
            tree->cycle = std::move(child->cycle);
            tree->children = std::move(child->children);
        }
    }

    return tree;
}

template <typename Real>
std::vector<int> MinimalCycleBasis<Real>::ExtractCycle(std::vector<Vertex*>& closedWalk)
{
    // TODO:  This logic was designed not to remove filaments after the
    // cycle deletion is complete.  Modify this to allow filament removal.

    // The closed walk is a cycle.
    int const numVertices = static_cast<int>(closedWalk.size());
    std::vector<int> cycle(numVertices);
    for (int i = 0; i < numVertices; ++i)
    {
        cycle[i] = closedWalk[i]->name;
    }

    // The clockwise-most edge is always removable.
    Vertex* v0 = closedWalk[0];
    Vertex* v1 = closedWalk[1];
    Vertex* vBranch = (v0->adjacent.size() > 2 ? v0 : nullptr);
    v0->adjacent.erase(v1);
    v1->adjacent.erase(v0);

    // Remove edges while traversing counterclockwise.
    while (v1 != vBranch && v1->adjacent.size() == 1)
    {
        Vertex* adj = *v1->adjacent.begin();
        v1->adjacent.erase(adj);
        adj->adjacent.erase(v1);
        v1 = adj;
    }

    if (v1 != v0)
    {
        // If v1 had exactly 3 adjacent vertices, removal of the CCW edge
        // that shared v1 leads to v1 having 2 adjacent vertices.  When
        // the CW removal occurs and we reach v1, the edge deletion will
        // lead to v1 having 1 adjacent vertex, making it a filament
        // endpoints.  We must ensure we do not delete v1 in this case,
        // allowing the recursive algorithm to handle the filament later.
        vBranch = v1;

        // Remove edges while traversing clockwise.
        while (v0 != vBranch && v0->adjacent.size() == 1)
        {
            v1 = *v0->adjacent.begin();
            v0->adjacent.erase(v1);
            v1->adjacent.erase(v0);
            v0 = v1;
        }
    }
    // else the cycle is its own connected component.

    return cycle;
}

template <typename Real>
typename MinimalCycleBasis<Real>::Vertex*
    MinimalCycleBasis<Real>::GetClockwiseMost(Vertex* vPrev, Vertex* vCurr) const
{
    Vertex* vNext = nullptr;
    bool vCurrConvex = false;
    std::array<Real, 2> dCurr, dNext;
    if (vPrev)
    {
        dCurr[0] = (*vCurr->position)[0] - (*vPrev->position)[0];
        dCurr[1] = (*vCurr->position)[1] - (*vPrev->position)[1];
    }
    else
    {
        dCurr[0] = static_cast<Real>(0);
        dCurr[1] = static_cast<Real>(-1);
    }

    for (auto vAdj : vCurr->adjacent)
    {
        // vAdj is a vertex adjacent to vCurr.  No backtracking is allowed.
        if (vAdj == vPrev)
        {
            continue;
        }

        // Compute the potential direction to move in.
        std::array<Real, 2> dAdj;
        dAdj[0] = (*vAdj->position)[0] - (*vCurr->position)[0];
        dAdj[1] = (*vAdj->position)[1] - (*vCurr->position)[1];

        // Select the first candidate.
        if (!vNext)
        {
            vNext = vAdj;
            dNext = dAdj;
            vCurrConvex = (dNext[0] * dCurr[1] <= dNext[1] * dCurr[0]);
            continue;
        }

        // Update if the next candidate is clockwise of the current
        // clockwise-most vertex.
        if (vCurrConvex)
        {
            if (dCurr[0] * dAdj[1] < dCurr[1] * dAdj[0]
                || dNext[0] * dAdj[1] < dNext[1] * dAdj[0])
            {
                vNext = vAdj;
                dNext = dAdj;
                vCurrConvex = (dNext[0] * dCurr[1] <= dNext[1] * dCurr[0]);
            }
        }
        else
        {
            if (dCurr[0] * dAdj[1] < dCurr[1] * dAdj[0]
                && dNext[0] * dAdj[1] < dNext[1] * dAdj[0])
            {
                vNext = vAdj;
                dNext = dAdj;
                vCurrConvex = (dNext[0] * dCurr[1] < dNext[1] * dCurr[0]);
            }
        }
    }

    return vNext;
}

template <typename Real>
typename MinimalCycleBasis<Real>::Vertex*
    MinimalCycleBasis<Real>::GetCounterclockwiseMost(Vertex* vPrev, Vertex* vCurr) const
{
    Vertex* vNext = nullptr;
    bool vCurrConvex = false;
    std::array<Real, 2> dCurr, dNext;
    if (vPrev)
    {
        dCurr[0] = (*vCurr->position)[0] - (*vPrev->position)[0];
        dCurr[1] = (*vCurr->position)[1] - (*vPrev->position)[1];
    }
    else
    {
        dCurr[0] = static_cast<Real>(0);
        dCurr[1] = static_cast<Real>(-1);
    }

    for (auto vAdj : vCurr->adjacent)
    {
        // vAdj is a vertex adjacent to vCurr.  No backtracking is allowed.
        if (vAdj == vPrev)
        {
            continue;
        }

        // Compute the potential direction to move in.
        std::array<Real, 2> dAdj;
        dAdj[0] = (*vAdj->position)[0] - (*vCurr->position)[0];
        dAdj[1] = (*vAdj->position)[1] - (*vCurr->position)[1];

        // Select the first candidate.
        if (!vNext)
        {
            vNext = vAdj;
            dNext = dAdj;
            vCurrConvex = (dNext[0] * dCurr[1] <= dNext[1] * dCurr[0]);
            continue;
        }

        // Select the next candidate if it is counterclockwise of the current
        // counterclockwise-most vertex.
        if (vCurrConvex)
        {
            if (dCurr[0] * dAdj[1] > dCurr[1] * dAdj[0]
                && dNext[0] * dAdj[1] > dNext[1] * dAdj[0])
            {
                vNext = vAdj;
                dNext = dAdj;
                vCurrConvex = (dNext[0] * dCurr[1] <= dNext[1] * dCurr[0]);
            }
        }
        else
        {
            if (dCurr[0] * dAdj[1] > dCurr[1] * dAdj[0]
                || dNext[0] * dAdj[1] > dNext[1] * dAdj[0])
            {
                vNext = vAdj;
                dNext = dAdj;
                vCurrConvex = (dNext[0] * dCurr[1] <= dNext[1] * dCurr[0]);
            }
        }
    }

    return vNext;
}

template <typename Real>
MinimalCycleBasis<Real>::Vertex::Vertex(int inName, std::array<Real, 2> const* inPosition)
    :
    name(inName),
    position(inPosition),
    visited(0)
{
}

template <typename Real>
bool MinimalCycleBasis<Real>::Vertex::operator< (Vertex const& vertex) const
{
    return name < vertex.name;
}

}
